package snucse.teamd;

import snucse.teamd.math.Vec2;
import java.util.*;


public class Arbiter {
	List<Contact> contacts;
	Body b1;
	Body b2;
	private int numOfContacts;
	float timeout;
	boolean valid=true;
	
	public List<Contact> getContacts() {
		return contacts;
	}

	public void setContacts(List<Contact> contacts) {
		this.contacts = contacts;
		numOfContacts = contacts.size();
	}

	public Arbiter(Body b1, Body b2){
		
		this.b1 = b1;
		this.b2 = b2;
	}
	
	public void UpdateContacts(){
		//?
	}
	
	public void ApplyImpulse()
	{
		if (!valid) return;
		valid=false;
	//pseudo-code first
		double[] J = new double[numOfContacts]; //J is the value of impulse. normal direction 
		
		//Suppose contact points are given

		Vec2[] v1i = new Vec2[numOfContacts];
		Vec2[] v1f = new Vec2[numOfContacts];
		Vec2[] v2i = new Vec2[numOfContacts];
		Vec2[] v2f = new Vec2[numOfContacts];
		Vec2[] r1 = new Vec2[numOfContacts];
		Vec2[] r2 = new Vec2[numOfContacts]; 
		
		//angular velocity
		Vec2 w1 = Vec2.scalar(new Vec2(0f,0f,1f), (double)b1.getAngularVelocity());
		Vec2 w2 = Vec2.scalar(new Vec2(0f,0f,1f), (double)b2.getAngularVelocity());
		
		//average friction coefficient, collision coefficient
		float avgMu = (float) Math.sqrt(b1.getFriction()*b2.getFriction());
		float avgE = (float)Math.sqrt(b1.getE()*b2.getE()); 
		
		//get I1, I2
		double invI1 = 0.001f;
		double invI2 = 0.001f;
		
		//get invM1, invM2
		double invM1 = 0.0f;
		double invM2 = 0.0f;
		
		Vec2 Jn ;
		Vec2 muJt;
		
		//calculate values for all contact points
		//two bodies are both objects
		for(int i = 0; i < numOfContacts; i++)
		{
			/*if (b1.envNormal==1) {
				b2.vel.setX(b2.vel.getX()/10);
				b2.vel.setY(b2.vel.getY()/10);
			}
			if (b2.envNormal==1) {
				b1.vel.setX(b1.vel.getX()/10);
				b1.vel.setY(b1.vel.getY()/10);
			}*/
			//initial velocity for b1 and b2 at the contact point
			/*
			v1i[i] = Vec2.plus(b1.getVel(), Vec2.crossVec(new Vec2(0, 0, b1.getAngularVelocity()), contacts.get(i).getR1())); 
			v2i[i] = Vec2.plus(b2.getVel(), Vec2.crossVec(new Vec2(0, 0, b2.getAngularVelocity()), contacts.get(i).getR2()));
			*/
			v1i[i] = b1.getVel(); 
			v2i[i] = b2.getVel();
			//get relative velocity of two bodies 
			Vec2 vrel = Vec2.minus(v1i[i], v2i[i]);
			
			//should get normal unit vector
			Vec2 n = contacts.get(i).getNormal(); 
			n.normalize();
			
			//should get tangential unit vector
			Vec2 t = Vec2.crossVec(Vec2.crossVec(n, vrel),n);
			t.normalize();
			
			r1[i] = contacts.get(i).getR1();//Vec2.minus(contacts.get(i).getR1(), b1.getCenterOfMass());//?
			r2[i] = contacts.get(i).getR2();//Vec2.minus(contacts.get(i).getR2(), b1.getCenterOfMass());//?
			
			//get I1, I2

			invI1 = 0.0f;
			invI2 = 0.0f;

			
			//get invM1, invM2
			invM1 = 0.0f;
			invM2 = 0.0f;
			
			if(!b1.isEnv && b2.isEnv)
			{
				 invI1 = 1/b1.getI();
				 invM1 = 1/b1.getMass();
			}
			else if(b1.isEnv && !b2.isEnv)
			{
				invI2 = 1/b2.getI();
				invM2 = 1/b2.getMass();
			}
			else if(!b1.isEnv && !b2.isEnv)
			{
				invI1 = 1/b1.getI();
				invI2 = 1/b2.getI();
			    invM1 = 1/b1.getMass();
				invM2 = 1/b2.getMass();
			}
			/*
			System.out.println("================================================");
			System.out.println("-(avgE+1.0)*Vec2.dot(vrel, n)	"+(-(avgE+1.0)*Vec2.dot(vrel, n)));
			System.out.println("invM1+invM2	"+invM1+invM2);

			System.out.println();
			System.out.print("Vec2.crossVec(r1[i], n)=	");
			Vec2.crossVec(r1[i], n).print();
			System.out.print("Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i])=	");
			Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i]).print();
			System.out.print("Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i]), invI1)=	");
			Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i]), invI1).print();
			System.out.println("Vec2.dot(n, Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i]), invI1))	"
					+Vec2.dot(n, Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i]), invI1)));
			
			System.out.println();
			System.out.print("Vec2.crossVec(r2[i], n)=	");
			Vec2.crossVec(r2[i], n).print();
			System.out.print("Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i])=	");
			Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i]).print();
			System.out.print("Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i]), invI2)=	");
			Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i]), invI2).print();
			System.out.println("Vec2.dot(n, Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i]), invI2))	"
					+Vec2.dot(n, Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i]), invI2)));
			System.out.println("================================================");
			*/
			
			J[i] = -(avgE+1.0)*Vec2.dot(vrel, n)/
					(invM1+invM2+
							Vec2.dot(n, Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r1[i], n), r1[i]), invI1))+
							Vec2.dot(n, Vec2.scalar(Vec2.crossVec(Vec2.crossVec(r2[i], n), r2[i]), invI2))
					);
			/*
			System.out.println("===============================================");
			System.out.println("Jn=	");
			Vec2.scalar(n, J[i]).print();
			System.out.println("muJt=	");
			System.out.println(Vec2.scalar(t, avgMu * J[i]));
			System.out.println("===============================================");
			*/
			
			Jn = Vec2.scalar(n, J[i]);
			muJt = Vec2.scalar(t, avgMu * J[i]);
			v1f[i] = Vec2.plus(v1i[i], Vec2.scalar(Vec2.plus(Jn, muJt), invM1));
			v2f[i] = Vec2.plus(v2i[i], Vec2.scalar(Vec2.minus(muJt, Jn), invM1));
			
			System.out.println("v1f[i]= "+v1f[i]+"	v2f[i]= "+v2f[i]);
			
			w1 = Vec2.plus(w1,
					Vec2.scalar(
							Vec2.crossVec(r1[i],
									/*Vec2.plus(
											b1.getCenterOfMass(),r1[i]),*/ 
											
											Vec2.plus(muJt, Jn)), 
											invI1));
			w2 = Vec2.plus(w2,
					Vec2.scalar(
							Vec2.crossVec(r2[i],
									/*Vec2.plus(
											b2.getCenterOfMass(), r2[i]),*/ 
											
											Vec2.minus(muJt, Jn)), 
											invI2));

			
		}
		Vec2 v1f_acc = new Vec2(0f,0f,0f);
		Vec2 v2f_acc = new Vec2(0f,0f,0f);
		for(int i=0; i<numOfContacts; i++)
		{
			v1f_acc = Vec2.plus(v1f_acc, v1f[i]);
			v2f_acc = Vec2.plus(v2f_acc, v2f[i]);
		}
		if(numOfContacts > 0)
		{
			//v1f_acc = Vec2.scalar(v1f_acc, 1/numOfContacts);
			//v2f_acc = Vec2.scalar(v2f_acc, 1/numOfContacts);
			
			w1 =Vec2.scalar(w1, 1/numOfContacts);
			w2 =Vec2.scalar(w2, 1/numOfContacts);
		}
		
		if(b1.isEnv && b2.isEnv)
		{
			b1.setVel(new Vec2());
			b1.setAngularVelocity(0f);
			b2.setVel(new Vec2());
			b2.setAngularVelocity(0f);
		}
		else if(!b1.isEnv && b2.isEnv)
		{
			b1.setVel(attenuation(v1f_acc));
			b1.setAngularVelocity(logarithm(w1.getZ()));
			b2.setVel(new Vec2());
			b2.setAngularVelocity(0f);
		}
		else if(b1.isEnv && !b2.isEnv)
		{
			b1.setVel(new Vec2());
			b1.setAngularVelocity(0f);
			b2.setVel(attenuation(v2f_acc));
			b2.setAngularVelocity(logarithm(w2.getZ()));
		}
		else
		{
			b1.setVel(attenuation(v1f_acc));
			b2.setVel(attenuation(v2f_acc));
		
			b1.setAngularVelocity(logarithm(w1.getZ()));
			b2.setAngularVelocity(logarithm(w2.getZ()));
		}
	}
	private Vec2 attenuation(Vec2 v)
	{
		if(Math.abs(v.getX()) < 1f)
		{
			if(Math.abs(v.getY()) < 0.1f)
				return new Vec2();
			else
				return new Vec2(0,v.getX());
		}
		else
		{
			return v;
		}
	}
	private float logarithm(double d)
	{
		if(d >= 0)
		{
			return (float)Math.log10(d+1);
		}
		else
		{
			return -(float)Math.log10(-d+1);
		}
	}
	
}
